import java.awt.Color;
import java.io.IOException;
import java.util.Random;

public class Agent {
	private double aktuelleGeschwindigkeit;
	private double maxGeschwindigkeit;
	private double positionX;
	private double positionY;
	private double zielX;
	private double zielY;
	private double aktuelleZeit;
	private double beschleunigung;
	private Color color;
	//private double previousX, previousY, x0, y0;
	private int fehlschlag; //Anzahl Fehlschl�ge von aktueller Position
	private GUI gui;
	private static final double WAHRSCHEINLICHKEIT_DIREKT = 0.3; // Wahrscheinlichkeit direkt in Richtung Ziel zu gehen
	private static final double GEWICHT_FEHLSCHLAG = 0.5; // das Gewicht der Anzahl Fehlschl�ge im Vergleich zur Entfernung zum Ziel beim Fitnesswert
	
	public static Agent createAgent(String[] daten, GUI gui){
		double[] datenDouble = new double[daten.length];
		for (int i = 0; i < daten.length; i++){
			datenDouble[i] = Double.parseDouble(daten[i]);
		}
		return new Agent(datenDouble[0], datenDouble[1], datenDouble[2], datenDouble[3], datenDouble[4], datenDouble[5], datenDouble[6], gui);
	}
	
	public Agent(double x0, double y0, double x1, double y1, double v0, double a, double vmax, GUI gui){
		this.aktuelleGeschwindigkeit = v0;
		this.maxGeschwindigkeit = vmax;
		this.positionX = x0;
		this.positionY = y0;
		this.zielX = x1;
		this.zielY = y1;
		this.beschleunigung = a;
		this.aktuelleZeit = 0;
		this.fehlschlag = 0;
		Random r = new Random();
		setColor(new Color(r.nextInt(255), r.nextInt(255), r.nextInt(255)));
		this.gui = gui;
	}
	
	public Agent(double x0, double y0, double x1, double y1, double v0, double a, double vmax, double t, GUI gui){
		this.aktuelleGeschwindigkeit = v0;
		this.maxGeschwindigkeit = vmax;
		this.positionX = x0;
		this.positionY = y0;
		this.zielX = x1;
		this.zielY = y1;
		this.beschleunigung = a;
		this.aktuelleZeit = t;
		this.fehlschlag = 0;
		Random r = new Random();
		setColor(new Color(r.nextInt(255), r.nextInt(255), r.nextInt(255)));
		this.gui = gui;
	}
	
	/**
	 * bewege Agent zu neuer Position, wird er unterwegs erwischt, bewegt er sich nicht
	 * @param posx neue Position X
	 * @param posy neue Position Y
	 * @param beschleunigung unterwegs Beschleunigen?
	 * @param hindernisse 
	 * @return true, wenn nicht erwischt, sonst false
	 */
	public boolean moveTo(double posx, double posy, int beschleunigung, Hindernis[] hindernisse){
		//TODO: Sonderf�lle hinzuf�gen (bei Beschleunigung bei Maximalgeschw. stoppen etc.)
		//System.out.println("moveTo");
		double previousX = positionX;
		double previousY = positionY;
	
		double zeit = 0;
		boolean erfolg = true;
		for (int i = 0; i < hindernisse.length; i++){
			erfolg = erfolg && !checkCollision(posx, posy, beschleunigung, hindernisse[i]);
			//System.out.println("erfolg = " + erfolg);
		}
		//System.out.println("Juhu, so weit komm ich");
		if (erfolg){
			zeit = getDauer(posx, posy, beschleunigung);
			if (!((aktuelleGeschwindigkeit == 0) && (beschleunigung == 0))){
				previousX = positionX;
				previousY = positionY;
//				System.out.println("von " + positionX + "//" + positionY + " nach " + posx + "//" + posy);
				positionX = posx;
				positionY = posy;
				if (hindernisse[0].getAbstand() > getDistance(hindernisse[0].getX(aktuelleZeit + zeit), 
									hindernisse[0].getY(aktuelleZeit + zeit))){
					System.out.println("Erwischt, aber das wird nicht erkannt");
				}
				gui.drawAgentAt(this, Math.round(posx), Math.round(posy), Math.round(previousX), Math.round(previousY));
			}
			aktuelleGeschwindigkeit += zeit * beschleunigung * this.beschleunigung;
			aktuelleZeit += zeit;
		}
		
		return erfolg;
	}
	
	/**
	 * kopiert den Agenten und gibt die Kopie zur�ck
	 */
	public Agent copy(){
		return new Agent(positionX, positionY, zielX, zielY, aktuelleGeschwindigkeit, beschleunigung, 
				maxGeschwindigkeit, aktuelleZeit, gui);
	}
	
	/**
	 * gibt den Fitnesswert des Agenten zur�ck. Fitness = Entfernung zum Ziel + Anzahl Fehlschl�ge * Gewicht
	 * @return fitness
	 */
	public double getFitness(){
		return getDistance(zielX, zielY) + (fehlschlag * GEWICHT_FEHLSCHLAG);
	}
	
	public double getZielX() {
		return zielX;
	}

	public double getZielY() {
		return zielY;
	}

	public double getAktuelleZeit() {
		return aktuelleZeit;
	}
	
	/**
	 * erh�ht Fehlschlag um 1
	 */
	public void incFehlschlag(){
		fehlschlag++;
	}
	
	public int getFehlschlag(){
		return fehlschlag;
	}
	
	/**
	 * minimale Zeit bis zum Ziel
	 */
	public double getBestzeit(){
		return aktuelleZeit + getDauer(zielX, zielY, 1);
	}
	
	public boolean zielErreicht(){
		return ((positionX == zielX) && (positionY == zielY));
	}
	
	/**
	 * projiziert den Punkt nach innerhalb, falls er ausserhalb ist. Daf�r wird auf der Gerade von
	 * Position zu Ziel die Entfernung vom Ziel zum Schnittpunkt der Geraden mit dem "Rahmen" genommen
	 * und zur�ckgerechnet. erst mit oben/unten, danach links/rechts. Auch k�rzen, falls Maximalgeschw.
	 * �berschritten werden w�rde bzw. 0 unterschritten w�rde
	 * 
	 * @param posx
	 * @param posy das Ziel
	 * @param beschleunigung
	 * @return g�ltiger Punkt
	 */
	private double[] projizierePunkt(double posx, double posy, int beschleunigung){
		double[] result = new double[2];
		double schnittX = 0;
		double schnittY = 0;
		boolean fehler = ((aktuelleGeschwindigkeit == 0) && (beschleunigung == -1)) ||
						((aktuelleGeschwindigkeit == maxGeschwindigkeit) && (beschleunigung == 1));
		double distMax = Double.MAX_VALUE;
		double a = 0;
		double b = 0;
		
//		System.out.println("Projektion");
//		System.out.println("positionX = " + positionX);
//		System.out.println("positionY = " + positionY);
//		System.out.println("posx = " + posx);
//		System.out.println("posy = " + posy);
//		System.out.println("beschleunigung = " + beschleunigung);
		//K�rze falls �ber Maximalgeschw. bzw. unter 0
		if (beschleunigung == 1){
			double tziel = getDauer(posx, posy, beschleunigung);
			double vziel = (beschleunigung == 1)? maxGeschwindigkeit : 0;
			double tmax = (vziel - aktuelleGeschwindigkeit) / (beschleunigung * this.beschleunigung);
			double t = tziel % tmax;
			//tziel wirft bei negativer Beschleunigung evtl. NaN aus
			if (Double.doubleToLongBits(t) == Double.doubleToLongBits(Double.NaN)){
				t = tmax/2;
			}
			if (t == 0){
				t = tmax;
			}
			double dist = aktuelleGeschwindigkeit * t + 0.5 *beschleunigung*this.beschleunigung*t*t;
			double[] punkt = polarZuNormal(posx, posy, dist);
			posx = punkt[0];
			posy = punkt[1];

			//teste ob jetzt ok
			if (Double.doubleToLongBits(getDauer(posx, posy, beschleunigung)) == Double.doubleToLongBits(Double.NaN)){
				System.out.println("ARRGGSS!!!");
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
//			System.out.println("K�rzung da Geschwindigkeit zu hoch/niedrig");
//			System.out.println("aktuelleGeschwindigkeit = " + aktuelleGeschwindigkeit);
//			System.out.println("tziel = " + tziel);
//			System.out.println("tmax = " + tmax);
//			System.out.println("t = " + t);
//			System.out.println("dist = " + dist);
//			System.out.println("posx = " + posx);
//			System.out.println("posy = " + posy);
		}
		
		//verhindere, dass Geschwindigkeit unter 0
		if (beschleunigung == -1){
			double tmax = (aktuelleGeschwindigkeit) / (this.beschleunigung);
//			System.out.println("aktuelleGeschwindigkeit = " + aktuelleGeschwindigkeit);
//			System.out.println("tmax = " + tmax);
			distMax = aktuelleGeschwindigkeit * tmax + 0.5 * this.beschleunigung * beschleunigung * tmax * tmax;
//			System.out.println("distMax = " + distMax);
			double dist = getDistance(posx, posy) % distMax/2;
			if (dist == 0){
				dist = distMax/2;
//				System.out.println("ge�ndert da 0");
			}
			double[] punkt = polarZuNormal(posx, posy, dist);
			posx = punkt[0];
			posy = punkt[1];
//			System.out.println("aktueller Punkt: " + positionX + "//" + positionY);
//			System.out.println("neuer Punkt: " + posx + "//" + posy);
//			System.out.println("Entfernung: " + getDistance(posx, posy));
//			System.out.println("Dauer bis dahin: " + getDauer(posx, posy, -1));
			if (getDistance(posx, posy) > distMax){
				System.out.println("�BERFLIEGER!!! getDistance = " + getDistance(posx, posy) + " distmax = "+ distMax);
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			if (aktuelleGeschwindigkeit - getDauer(posx, posy, -1)*this.beschleunigung < 0){
				System.out.println("Punkt falsch!");
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
		}
		
		//Schneide ab, falls ausserhalb
		//Bestimme Gerade mit y = ax + b
		if ((posx < 0) || (posx > 100) || (posy < 0) || (posy > 100)){
			a = (positionY - posy) / (positionX - posx); 
			if (positionX == posx){
				System.out.println("projekt posx = positionX");
			}
			if (positionY == posy){
				System.out.println("projekt posy = positionY");
			}
			b = posy - (a * posx);
		}
		
		//x > 100 oder x < 0 
		if ((posx > 100) || (posx < 0)){
			schnittX = (posx > 100)?100:0;
			schnittY = a * schnittX + b;
			if (positionX == schnittX){
				fehler = true;
			} else {
				//oben versprochene Projektion
				posx = Math.abs(schnittX - ((posx - schnittX)%(schnittX - positionX)));
				posy = a * posx + b;
			}
//			System.out.println("Beschneidung da x > 100 oder < 0");
		}
		
		// y > 100 oder y > 0
		if ((posy > 100) || (posy < 0)){
			schnittY = (posy > 100)?100:0;
			schnittX = (schnittY - b) / a;
			if (positionY == schnittY){
				fehler = true;
			} else {
				//oben versprochene Projektion
				posy = Math.abs(schnittY - ((posy - schnittY)%(schnittY - positionY)));
				posx = (posy - b)/a;
			}
//			System.out.println("Beschneidung da y > 100 oder < 0");
		}
		
		if (fehler){
			result[0] = -1;
			result[1] = -1;
		} else {
			result[0] = posx;
			result[1] = posy;
		}
		if (Double.doubleToLongBits(result[0]) == Double.doubleToLongBits(Double.NaN)){
			System.out.println("posx NaN");
		}
		if (Double.doubleToLongBits(result[1]) == Double.doubleToLongBits(Double.NaN)){
			System.out.println("posy NaN");
		}
		if (distMax < getDistance(posx, posy)){
			System.out.println("UNDERCOVER�BERFLIEGER");
		}
		return result;
		
	}
	
	/**
	 * gibt den Punkt zur�ck, der die gegebene Entfernung von aktueller Position hat in Richtung 
	 * des gegebenen Punktes
	 * @param posx
	 * @param posy der Punkt
	 * @param dist die Enternung
	 * @return den Punkt
	 */
	private double[] polarZuNormal(double posx, double posy, double dist){
		double betrag = getDistance(posx, posy); //verhindere Rundungsfehler, die Entfernung erhöhen
		double richtungX = (posx - positionX) / betrag;
		double richtungY = (posy - positionY) / betrag;
		double[] result = new double[2];
		result[0] = positionX + (richtungX * dist);
		result[1] = positionY + (richtungY * dist);
		return result;
	}
	
	/**
	 * Bewegt den Agenten zu einem neuen Punkt in eine zuf�llige Richtung
	 * 
	 * @return true bei Erfolg und false bei nicht Erfolg.
	 */
	public boolean moveToRandomDirection(Hindernis[] hindernis){
		return moveToRandomPoint(hindernis, true);
	}
	/**
	 * Bewegt den Agenten zu einem neuen Punkt in Richtung des Ziels
	 * 
	 * @return true bei Erfolg und false bei nicht Erfolg.
	 */
	public boolean moveToRandomDistance(Hindernis[] hindernis){
		return moveToRandomPoint(hindernis, false);
	}
	
	
	/**
	 * Bewegt den Agenten zu einem neuen Punkt, entweder in eine zuf�llige Richtung oder in Richtung Ziel
	 * 
	 * @return true bei Erfolg und false bei nicht Erfolg.
	 */
	public boolean moveToRandomPoint(Hindernis[] hindernis, boolean randomDirection){
		//double pointX = 0;
		//double pointY = 0;
		double directionX = 0;
		double directionY = 0;
		double stepLength = 0;
		boolean success = false;
//		boolean test = false;
		
		//Bestimme die Richtung, in die der Agent sich bewegen soll:
		if (Math.random() < WAHRSCHEINLICHKEIT_DIREKT || !randomDirection)
		{
			directionX = getEinheitsVectorX(zielX - positionX, zielY - positionY);
			directionY = getEinheitsVectorY(zielX - positionX, zielY - positionY);
//			test = true;
//			System.out.println("directionX = " + directionX);
//			System.out.println("directionY = " + directionY);
		} else {
			directionX = Math.random();
			if (directionX == 1){
				directionX -= 0.001;
			}
			if (directionX == 0){
				directionX += 0.001;
			}
			directionY = Math.sqrt(1 - Math.pow(directionX, 2));
			
			double tmpX = Math.random();
			if (tmpX < 0.25){
				directionX = -directionX;
			}
			if (tmpX >= 0.5 && tmpX < 0.75){
				directionY = -directionY;
			}
			if (tmpX >= 0.75){
				directionX = -directionX;
				directionY = -directionY;
			}
		}
		if (Math.random() < WAHRSCHEINLICHKEIT_DIREKT)
		{
			stepLength = getDistance(zielX,zielY);
//			if (test) System.out.println("direktfahrt zum Ziel");
		} else {
			//zuf�llige Schrittweite mindestens 1/100 der Entfernung zum Ziel
			stepLength = (getDistance(zielX,zielY) - getDistance(zielX, zielY)/100) * Math.random() +getDistance(zielX, zielY)/100;
		}
//		System.out.println("stepLength = " + stepLength);
		int neueBeschleunigung = 0;
		double tmp = Math.random();
		if (tmp > 0.66) {
			neueBeschleunigung = -1;
		}
		if (tmp <= 0.66 && tmp > 0.33) {
			neueBeschleunigung = 1;
		}
//		System.out.println("neueBeschleunigung = " + neueBeschleunigung);
		double[] point = projizierePunkt(positionX + directionX * stepLength ,positionY + directionY * stepLength, neueBeschleunigung);
		//System.out.println("Bis hier alles gut");
		//point[0] = positionX + directionX * stepLength;
		//point[1] = positionY + directionY * stepLength;
		if (point[0] == -1) {
			success = false;
		} else {
//			System.out.println(" von " + positionX + "/" + positionY + " nach " + point[0] + "/" + point[1]);
			try {
				if (Double.doubleToLongBits(positionX) == Double.doubleToLongBits(Double.NaN)){
					System.out.println("verdammt");
					System.in.read();
				}
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
			success = moveTo(point[0], point[1], neueBeschleunigung, hindernis); 
		}
		return success;
	}
	
	private double getEinheitsVectorX(double posX, double posY){
		return posX / (Math.sqrt(posX*posX + posY * posY));
	}
	private double getEinheitsVectorY(double posX, double posY){
		return posY / (Math.sqrt(posX*posX + posY * posY));
	}

	/**
	 * gibt die Entfernung des Agenten zu einem Punkt zur�ck
	 * @param posx x-Koordinate des Punktes
	 * @param posy y-Koordinate
	 * @return Entfernung (2-Norm)
	 */
	private double getDistance(double posx, double posy){
		return Math.sqrt(Math.pow(posx - positionX, 2) + Math.pow(posy - positionY, 2));
	}

	/**
	 * Pr�ft ob Agent auf dem Weg von aktueller Position mit aktueller Geschwindigkeit und Beschleunigung
	 * zu gegebener neuen Position von gegebenem Hindernis erwischt wird 
	 * @param posx x-Koordinate des Ziels
	 * @param posy y-Koordinate des Ziels
	 * @param beschleunigung ob Agent bei Bewegung beschleunigt, bremst oder konstante Geschwindigkeit hat
	 * 				beschleunigung in {-1, 0, 1}
	 * @param hindernis das Hindernis welches gepr�ft wird
	 * @return wird auf dem Weg zum Ziel erwischt
	 */
	private boolean checkCollision(double posx, double posy, int beschleunigung, Hindernis hindernis){
		//System.out.println("checkCollision");
		boolean result = false;
		double t = aktuelleZeit;
		double talt = aktuelleZeit;
//		System.out.println("checkStart");
		double endzeit = aktuelleZeit + getDauer(posx, posy, beschleunigung);
		if ((beschleunigung ==0) && (aktuelleGeschwindigkeit == 0)){
			posx = positionX;
			posy = positionY;
		}
		
		//System.out.println("aktuelleZeit = " + aktuelleZeit);
		//System.out.println("dauer = " + getDauer(posx, posy, beschleunigung));	
		
		// Kollisison zum Startzeitpunkt
		result = result || kontrolliereAbstand(aktuelleZeit, hindernis, beschleunigung, positionX, positionY);
		//System.out.println("starterwischt = " + result);
		
		// Kollisison zum Endzeitpunkt
		result |= (hindernis.getAbstand() > Math.sqrt(Math.pow(posx - hindernis.getX(endzeit), 2) + Math.pow(posy - hindernis.getY(endzeit), 2)));
		//System.out.println(result);
		
		// Kollision zu Zeitpunkten mit Nullstellen
		t = aktuelleZeit;
		talt = t-1;
//		System.out.println("nullX beginn");
		while ((t < endzeit) && (t >= aktuelleZeit) && (talt < t)){
//			System.out.println("nullX  t = " + t);
			talt = t;
			t = getNullstelleX(hindernis, beschleunigung, t, posx, posy);
			if ((t > aktuelleZeit) && (t < endzeit)){
				result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
			}
		}
//		System.out.println(result);
		
		t = aktuelleZeit;
		talt = t-1;
//		System.out.println("nullY beginn");
		while ((t < endzeit) && (t >= aktuelleZeit) && (talt < t)){
			//System.out.println("nullY");
			talt = t;
			t = getNullstelleY(hindernis, beschleunigung, t, posx, posy);
			if ((t > aktuelleZeit) && (t < endzeit)){
				result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
			}
		}
//		System.out.println(result);
		
		// Kollision zu Zeitpunkten mit Extremstellen
		t = aktuelleZeit;
//		System.out.println("extremX beginn");
		while ((t < endzeit) && (t >= aktuelleZeit)){
			//System.out.println("extremX   t = " + t + "  endzeit = " + endzeit);
			t = hindernis.getExtremstelleX(t);
			if ((t > aktuelleZeit) && (t < endzeit)){
				result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
			}
		}
//		System.out.println(result);
		
		t = aktuelleZeit;
//		System.out.println("extremY beginn");
		while ((t < endzeit) && (t >= aktuelleZeit)){
			//System.out.println("extremY");
			t = hindernis.getExtremstelleY(t);
			if ((t > aktuelleZeit) && (t < endzeit)){
				result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
			}
		}
//		System.out.println(result);
		
		// Kollision zu Zeitpunkten mit gleicher Steigung
		t = getGleicheSteigungX(hindernis, beschleunigung, t, posx, posy)[0];
		if ((t > aktuelleZeit) && (t < endzeit)){
			result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
		}
		
		t = getGleicheSteigungX(hindernis, beschleunigung, t, posx, posy)[1];
		if ((t > aktuelleZeit) && (t < endzeit)){
			result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
		}
		
		t = getGleicheSteigungY(hindernis, beschleunigung, t, posx, posy)[0];
		if ((t > aktuelleZeit) && (t < endzeit)){
			result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
		}
		
		t = getGleicheSteigungY(hindernis, beschleunigung, t, posx, posy)[1];
		if ((t > aktuelleZeit) && (t < endzeit)){
			result |= kontrolliereAbstand(t, hindernis, beschleunigung, posx, posy);
		}
		//System.out.println("checkCollision ist super");
		
		//System.out.println(result);
		
		return result;
	}
	
	private boolean kontrolliereAbstand(double t, Hindernis hindernis, int beschleunigung, double posx, double posy){
		double agentX = 0.5 * t * t * beschleunigung * getBeschleunigungX(posx, posy) 
				+ t * getGeschwindigkeitX(posx, posy) + positionX;
		double agentY = 0.5 * t * t * beschleunigung * getBeschleunigungY(posx, posy) 
				+ t * getGeschwindigkeitY(posx, posy) + positionY;
		double abstand = Math.sqrt(Math.pow(hindernis.getX(t) - agentX, 2) + 
				Math.pow(hindernis.getY(t) - agentY, 2));
//		System.out.println("geschwX = " + getGeschwindigkeitX(posx, posy) + " geschwY = " + getGeschwindigkeitY(posx, posy));
//		System.out.println("agX = " + agentX + " agY = " + agentY);
//		System.out.println("abstand = " + abstand + "   hindernis = " + hindernis.getAbstand());
		return abstand < hindernis.getAbstand();
	}
	
	/**
	 * gibt die nächste Nullstelle von (xAgent - xHindernis) zurück, die größer als startzeit ist (nicht gleich!)
	 * @param hindernis das Hindernis mit dem verglichen wird
	 * @param beschleunigung ob Agent beschleunigt, bremst oder Geschwindigkeit konstant (0, -1, 1)
	 * @param startzeit ab wann geprüft wird
	 * @return die nächste Nullstelle, -1 wenn keine gefunden wurde
	 */
	private double getNullstelleX(Hindernis hindernis, int beschleunigung, double startzeit, double zielX, double zielY){
		//TODO: anpassen (Tim)
		//Wir vergleichen solange Sägezähne des Hindernisses mit dem Agenten bis wir eine Nullstelle finden (TimeOut!)
		boolean nullstelle = false;
		double xmax, xmin, tmax,a,b;
		double startzeitH = startzeit;
		double t = 0;
		double t1 = 0;
		double t2 = 0;
		double p = 0;
		double z = 0;
		double q = 0;
//		double differenz = 0;
		if (getX(zielX, zielY, beschleunigung, startzeitH) == hindernis.getX(startzeitH)){
			startzeitH = hindernis.getExtremstelleX(startzeitH);
			if (startzeitH > getDauer(zielX, zielY, beschleunigung)){
				nullstelle = true;
				t = -1;
			}
		}
		
		while (!nullstelle) {
			tmax = hindernis.getExtremstelleX(startzeitH);
			if (tmax == Double.MAX_VALUE){
				tmax = aktuelleZeit + getDauer(zielX, zielY, beschleunigung) + 1;
			}
			xmin = hindernis.getX(startzeitH);
			xmax = hindernis.getX(tmax);
			
			if (tmax <= startzeitH){
				System.out.println("verkehrte Welt, startzeit >= endzeit O.o  startzeitH = " + startzeitH + " tmax = " + tmax);
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			//Das ganze in Geraden Gleichung a*t+b
			a = (xmin-xmax)/(startzeitH-tmax);
			if (Double.doubleToLongBits(a) == Double.doubleToLongBits(Double.NaN)){
				System.out.println("getNullX a = NaN");
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			b = xmin - (a * startzeitH);
			
			//Und nun den dreckigen Agenten:
			//Steigung gleich:
			z = (beschleunigung * getBeschleunigungX(zielX,zielY)/2); //Faktor vor t^2
			
			if ((beschleunigung == 0) || (z == 0)){
				//jo, das is voll die gerade alter
				if ((getGeschwindigkeitX(zielX,zielY) - a) == 0){
					//konstanter Abstand
				} else {
					t = ((b-positionX)/(getGeschwindigkeitX(zielX,zielY) - a));
					nullstelle = (t != startzeit); //nur beenden, wenn das nicht der startpunkt war
				}
			} else {
				//Kurvig wie bei Pamela gehts hier zu :)
//				if ((getBeschleunigungX(zielX,zielY)) == 0){
//					//kosntanter Abstand? hm glaube nicht: pr�fen
//				}
				p = (getGeschwindigkeitX(zielX,zielY) - a);
				q = (positionX - b);
				if ((Math.pow((p/(2*z)),2) - q/z ) < 0) {
					//div 0, wieder konstant? hm
				} else {
					t1 = (p / (-2*z)) + Math.sqrt((Math.pow((p/(2*z)),2) - q/z));
					t2 = (p / (-2*z)) - Math.sqrt((Math.pow((p/(2*z)),2) - q/z));
					//ist nicht immer t1 der h�here?
					t = Math.max(t1, t2);
					if (Double.doubleToLongBits(t) == Double.doubleToLongBits(Double.NaN)){
						System.out.println("in nullX t = NaN");
						try {
							System.in.read();
						} catch (IOException e) {
							// TODO Auto-generated catch block
							e.printStackTrace();
						}
					}
					nullstelle = (t != startzeit); //nur beenden, wenn das nicht der startpunkt war
				}
			}
			
			//TODO: warum wollte ich das machen?? xmax wird doch eh �berschrieben?
			//Wir machen einen auf iti, keine Variablen verschwenden!
			//xmax = xmax - this.getX();
			
			//wenn nichts gefunden suchen wir im nächsten Sägezahn...
//			if (differenz > tmax - startzeitH){
//				System.out.println("keine gleichm��ige Steigung in nullX");
//			}
//			differenz = tmax - startzeitH;
			startzeitH = tmax;
			if (startzeitH > getDauer(zielX,zielY,beschleunigung) && !nullstelle)
			{
				t = -1;
				nullstelle = true; //Nicht wirklich aber wir m�gen keine Endlosschleifen. 
			}
//			System.out.println("Differenz = " + differenz);
		}
//		System.out.println("nullX Schleife durch");
				
		return t;
	}
	
	/**
	 * wie getNullstelleX, nur für Y
	 * @param hindernis
	 * @param beschleunigung
	 * @param startzeit
	 * @return
	 */
	private double getNullstelleY(Hindernis hindernis, int beschleunigung, double startzeit, double zielX, double zielY){
		//TODO: anpassen (Tim)
		//Wir vergleichen solange Sägezähne des Hindernisses mit dem Agenten bis wir eine Nullstelle finden (TimeOut!)
		boolean nullstelle = false;
		double ymax, ymin, tmax,a,b;
		double startzeitH = startzeit;
		double t = 0;
		double t1 = 0;
		double t2 = 0;
		double p = 0;
		double z = 0;
		double q = 0;
		if (getY(zielX, zielY, beschleunigung, startzeitH) == hindernis.getY(startzeitH)){
			startzeitH = hindernis.getExtremstelleY(startzeitH);
		}
		if (startzeitH == Double.MAX_VALUE){
			nullstelle = true;
			t = -1;
		}
		
		while (!nullstelle) {
			tmax = hindernis.getExtremstelleY(startzeitH);
			if (tmax == Double.MAX_VALUE){
				tmax = aktuelleZeit + getDauer(zielX, zielY, beschleunigung) + 1;
			}
			ymax = hindernis.getY(tmax);
			ymin = hindernis.getY(startzeitH);
			if (tmax <= startzeitH){
				System.out.println("verkehrte Welt, startzeit >= endzeit O.o  startzeitH = " + startzeitH + " tmax = " + tmax);
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			//Das ganze in Geraden Gleichung a*t+b
			a = (ymin-ymax)/(startzeitH-tmax);
			if (Double.doubleToLongBits(a) == Double.doubleToLongBits(Double.NaN)){
				System.out.println("getNullX a = NaN");
				try {
					System.in.read();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			b = ymin - (a * startzeitH);
			
			//Und nun den dreckigen Agenten:
			//Steigung gleich:
			z = (beschleunigung * getBeschleunigungY(zielX,zielY)/2); //Faktor vor t^2
			
			if ((beschleunigung == 0) || (z == 0)){
				//jo, das is voll die gerade alter
				//TODO: getGeschwindigkeitX
				if ((getGeschwindigkeitY(zielX,zielY) - a) == 0){
					//konstanter Abstand
				} else {
					t = ((b-positionY)/(getGeschwindigkeitY(zielX,zielY) - a));
					nullstelle = (t != startzeit); //nur beenden, wenn das nicht der startpunkt war
				}
			} else {
				//Kurvig wie bei Pamela gehts hier zu :)
				if ((getBeschleunigungY(zielX,zielY)) == 0){
					//kosntanter Abstand? hm glaube nicht: pr�fen
				}
				p = (getGeschwindigkeitY(zielX,zielY) - a);
				q = (positionY - b);
				if (((Math.pow((p/(2*z)),2) - q/z ) < 0)) {
					//div 0, wieder konstant? hm
				} else {
					t1 = (p / (-2*z)) + Math.sqrt((Math.pow((p/(2*z)),2) - q/z));
					t2 = (p / (-2*z)) - Math.sqrt((Math.pow((p/(2*z)),2) - q/z));
					//ist nicht immer t1 der h�here?
					t = Math.max(t1, t2);
					if (Double.doubleToLongBits(t) == Double.doubleToLongBits(Double.NaN)){
						System.out.println("in nullX t = NaN");
						try {
							System.in.read();
						} catch (IOException e) {
							// TODO Auto-generated catch block
							e.printStackTrace();
						}
					}
					nullstelle = (t != startzeit); //nur beenden, wenn das nicht der startpunkt war
				}
			}
			
			//TODO: warum wollte ich das machen?? xmax wird doch eh �berschrieben?
			//Wir machen einen auf iti, keine Variablen verschwenden!
			//xmax = xmax - this.getX();
			
			//wenn nichts gefunden suchen wir im nächsten Sägezahn...
			startzeitH = tmax;
			if (startzeitH > aktuelleZeit + getDauer(zielX,zielY,beschleunigung) && !nullstelle)
			{
				t = -1;
				nullstelle = true; //Nicht wirklich aber wir m�gen keine Endlosschleifen. 
			}
			
		}
				
		return t;
	}
	
	/**
	 * gibt die n�chste Stelle nach Startzeit an, wo die Geschwindigkeit in X-Richtung gleich
	 * die Geschwindigkeit des Hindernis in X-Richtung ist 
	 * @param hindernis das Hindernis
	 * @param beschleunigung ob Agent beschleunigt oder abbremst (-1, 1), bei konstant wird diese Funktion
	 * 						nicht benutzt
	 * @param startzeit 
	 * @return n�chste Gleichheit von Geschwindigkeit in X-Richtung, wenn nicht existent -1
	 */
	private double[] getGleicheSteigungX(Hindernis hindernis, int beschleunigung, double startzeit, double posx, double posy){
		//TODO: anpassen(Peter)
		//Finde t f�r das gilt startgeschwAgent + t*beschleunigungAgent = (-)beschleunigungHinderniss
		double steigHind = Math.abs(hindernis.getGeschwindigkeitX());
		double result[] = new double[2];
		//t1/2 sind Punkte wo Betrag der Geschwindigkeit gleich, pr�fe danach welcher der richtige ist
		result[0] = (steigHind - getGeschwindigkeitX(posx, posy))/(beschleunigung * getBeschleunigungX(posx,posy));
		result[1] = (-steigHind - getGeschwindigkeitX(posx, posy))/(beschleunigung * getBeschleunigungX(posx, posy));
		return result;
	}
	
	/**
	 * wie oben, nur Y
	 * @param hindernis
	 * @param beschleunigung
	 * @param startzeit
	 * @return
	 */
	private double[] getGleicheSteigungY(Hindernis hindernis, int beschleunigung, double startzeit, double posx, double posy){
		//TODO: anpassen(Peter)
		//Finde t f�r das gilt startgeschwAgent + t*beschleunigungAgent = (-)beschleunigungHinderniss
		double steigHind = Math.abs(hindernis.getGeschwindigkeitY());
		double result[] = new double[2];
		//t1/2 sind Punkte wo Betrag der Geschwindigkeit gleich, pr�fe danach welcher der richtige ist
		result[0] = (steigHind - getGeschwindigkeitY(posx, posy))/(beschleunigung * getBeschleunigungY(posx,posy));
		result[1] = (-steigHind - getGeschwindigkeitY(posx, posy))/(beschleunigung * getBeschleunigungY(posx,posy));
		return result;
	}
	
	/**
	 * Startgeschwindigkeit im x-Bereich bei Bewegungung zu gegebenem Punkt
	 * im Prinzip ein Cosinus (Ankathete / Hypothenuse)
	 * @param x
	 * @param y
	 * @return
	 */
	private double getGeschwindigkeitX(double x, double y){
		
		double result = (y - positionY)/(Math.sqrt(Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2))) 
						*aktuelleGeschwindigkeit;
		if ((Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2) == 0)){
			result = 0; //scheinbar schon im Ziel
		}
		return result;
	}
	/**
	 * Startgeschwindigkeit im y-Bereich bei Bewegung zu gegebenem Punkt
	 * im Prinzip ein Sinus (Gegenkathete / Hypothenuse)
	 * @param x
	 * @param y
	 * @return
	 */
	private double getGeschwindigkeitY(double x, double y){
		double result =  (x - positionX)/(Math.sqrt(Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2))) 
						*aktuelleGeschwindigkeit;
		if ((Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2) == 0)){
			result = 0; //scheinbar schon im Ziel
		}
		return result;
	}
	
	/**
	 * Beschleunigung im x-Bereich bei Bewegung zu gegebenem Punkt, 
	 * muss bei bremsen mit -1 multipliziert werden
	 * @param x
	 * @param y
	 * @return
	 */
	private double getBeschleunigungX(double x, double y){
		double result =  (y - positionY)/(Math.sqrt(Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2)))
					*beschleunigung;
		if ((Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2) == 0)){
			result = 0; //scheinbar schon im Ziel
		}
		return result;
	}
	
	/**
	 * Beschleunigung im y-Bereich bei Bewegung zu gegebenem Punkt, 
	 * muss bei bremsen mit -1 multipliziert werden
	 * @param x
	 * @param y
	 * @return
	 */
	private double getBeschleunigungY(double x, double y){
		double result = (x - positionX)/(Math.sqrt(Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2)))
					*beschleunigung;
		if ((Math.pow(positionX - x, 2) + Math.pow(positionY - y, 2) == 0)){
			result = 0; //scheinbar schon im Ziel
		}
		return result;
	}
	
	/**
	 * Dauer der Bewegung zu Punkt unter Annahme, das nicht erwischt wird
	 * @param x-Koordinate des Ziels
	 * @param y-Koordniate des Ziels
	 * @param beschleunigung
	 * @return
	 */
	private double getDauer(double x, double y, int beschleunigung){
		double a = 0;
		double strecke = getDistance(x,y);
		double zeit = 0;
		double zeit1 = 0;
		double zeit2 = 0;
		
		double tmax = aktuelleGeschwindigkeit/this.beschleunigung;
		double streckeMax = aktuelleGeschwindigkeit*tmax + this.beschleunigung*beschleunigung*0.5*tmax*tmax;
//		System.out.println("x = " + x + "  y = " + y + "  beschl = " + beschleunigung);
//		System.out.println("strecke = " + strecke + "  aktuelleGeschw = " + aktuelleGeschwindigkeit);
		if (beschleunigung == 0){
			//wenn geschwindigkeit = 0, warte entfernung in sekunden
			if (aktuelleGeschwindigkeit == 0){
				zeit = strecke;
			} else {
				zeit = strecke / aktuelleGeschwindigkeit;
			}
		} else {
			a = beschleunigung * this.beschleunigung;
			//pq-Formel, w�hle positives Ergebnis
//			zeit = Math.max(-aktuelleGeschwindigkeit / a
//						+ Math.sqrt(Math.pow(aktuelleGeschwindigkeit /a, 2) 
//						+ 2*strecke/a),
//						-aktuelleGeschwindigkeit / a
//						- Math.sqrt(Math.pow(aktuelleGeschwindigkeit /a, 2) 
//						+ 2*strecke/a));
			zeit1 = -aktuelleGeschwindigkeit / a
					+ Math.sqrt(Math.pow(aktuelleGeschwindigkeit /a, 2) 
					+ 2*strecke/a);
			zeit2 = -aktuelleGeschwindigkeit / a
					- Math.sqrt(Math.pow(aktuelleGeschwindigkeit /a, 2) 
					+ 2*strecke/a);
			if (Math.min(zeit1, zeit2) >= 0){
				zeit = Math.min(zeit1, zeit2);
			} else {
				zeit = Math.max(zeit1, zeit2);
			}
		}
		if (Double.doubleToLongBits(zeit) == Double.doubleToLongBits(Double.NaN)){
			System.out.println("getDauer macht NaN, beschleunigung = " + beschleunigung);
			System.out.println("maxStrecke = " + streckeMax + " strecke = " + strecke);
			try {
				System.in.read();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		if (zeit < 0){
//			System.out.println("zeit bis zu einem Punkt soll negativ sein O.o zeit = " + zeit);
//			System.out.println("a = " + a + " aktuellG = " + aktuelleGeschwindigkeit + " beschleunigung = " + beschleunigung);
		}
		return zeit;
	}

	/**
	 * @param color the color to set
	 */
	public void setColor(Color color) {
		this.color = color;
	}

	/**
	 * @return the color
	 */
	public Color getColor() {
		return color;
	}
	
	public double getStartX() {
		return positionX;	
	}
	
	public double getStartY() {
		return positionY;		
	}
	
	private double getX(double posx, double posy, int beschleunigung, double t){
		return positionX + getGeschwindigkeitX(posx, posy) * t + getBeschleunigungX(posx, posy)*beschleunigung*t*t; 
	}
	
	private double getY(double posx, double posy, int beschleunigung, double t){
		return positionY + getGeschwindigkeitY(posx, posy) * t + getBeschleunigungY(posx, posy)*beschleunigung*t*t; 
	}
	
	
}
